/*
 * Copyright (c) 2006-2021, Halo team
 *
 * SPDX-License-Identifier: GPLv3
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-09-11     Eywen        the first version
 */

/**
  ******************************************************************************
  * @file    drv_pid.c
  * @author  Eywen
  * @brief   实现pid控制逻辑
  *
  @verbatim
  ==============================================================================
                        ##### How to use this driver #####
  ==============================================================================

  */

#include "drv_pid.h"
#include "../common.h"

static float pid_output_get(pid_control_t *pid);
static bool pid_init(pid_control_t *pid, float kp, float ki,
                     float kd, float max, float min,...);
static float pid_positon_run_once(pid_control_t *pid, float cur);
static float pid_diff_get(pid_control_t *pid);
static void pid_para_set(pid_control_t *pid, pid_para_t pid_para);
static void pid_target_set(pid_control_t *pid, float target);
static float pid_target_get(pid_control_t *pid);
static void pid_limit_set(pid_control_t *pid, pid_limit_t limit);
static pid_limit_t pid_limit_get(pid_control_t *pid);

const static pid_ops_t s_pid_position_ops =
{
    .init = pid_init,
    .diff = pid_diff_get,
    .run_once = pid_positon_run_once,
    .output = pid_output_get,
    .para_set = pid_para_set,
    .target_set = pid_target_set,
    .target = pid_target_get,
    .limit = pid_limit_get,
    .limit_set = pid_limit_set,
};



bool pid_register(pid_control_t *pid, pid_type_t pid_type)
{
    ASSERT_NULL(pid);

    pid->type = pid_type;

    switch ((int)pid_type) {
    case (pid_type_position):
        do{
            pid->ops = &s_pid_position_ops;
            pid->pid_type_base = NULL;
        }while(0);
        break;
    default:
        return false;
    }

    return true;
}

/*
pid_type_position:
    pid_init(pid_control_t *pid, float kp, float ki,
              float kd, float max, float min)
*/
bool pid_init(pid_control_t *pid, float kp, float ki,
              float kd, float max, float min,...)
{
    ASSERT_NULL(pid);

    pid->pid_base.err = 0;
    pid->pid_base.integral = 0;
    pid->pid_base.last_err = 0;
    pid->output = 0;
    pid->target = 0;
    pid->limit.max = max;
    pid->limit.min = min;
    pid->pid_para.kp = kp;
    pid->pid_para.ki = ki;
    pid->pid_para.kd = kd;

    switch ((int)pid->type) {
    case (pid_type_position):
        goto pid_type_position_init;
        break;
    default:
        return false;
    }

pid_type_position_init:
    do{
        //
    }while(0);
    return true;
}

/* 创建一个pid控制器 */
pid_control_t *pid_control_create(pid_type_t type)
{
    pid_control_t *pid_control;

    pid_control = MEM_MALLOC(sizeof (pid_control_t));
    if(pid_control == NULL){
        debug_error("Insufficient system memory");
        goto err;
    }

    MEM_MEMSET(pid_control, 0, sizeof(pid_control_t));

    switch (type) {
    case (pid_type_position):
        break;
    default:
        debug_error("PID type error");
        goto err;
        break;
    }

    if(false == pid_register(pid_control, type)){
        debug_error("pid register fail");
        goto err;
    }

    debug_info("pid creater success");
    return pid_control;
err:
    if(pid_control != NULL){
        MEM_FREE(pid_control);
    }

    return NULL;
}

float pid_output_get(pid_control_t *pid)
{
    ASSERT_NULL(pid);

    return pid->output;
}

static inline float pid_output_limit(pid_control_t *pid, float output)
{
    ASSERT_NULL(pid);

    if(output > pid->limit.max){
        return pid->limit.max;
    }
    if(output < pid->limit.min){
        return pid->limit.min;
    }
    return output;
}

void pid_para_set(pid_control_t *pid, pid_para_t pid_para)
{
    ASSERT_NULL(pid);

    pid->pid_para = pid_para;
}

float pid_diff_get(pid_control_t *pid)
{
    ASSERT_NULL(pid);

    return pid->pid_base.err;
}

float pid_positon_run_once(pid_control_t *pid, float cur)
{
    float output;

    pid->pid_base.err = pid->target - cur;
    pid->pid_base.integral += pid->pid_base.err;

    output = pid->pid_para.kp*pid->pid_base.err +
            pid->pid_para.ki*pid->pid_base.integral +
            pid->pid_para.kd*(pid->pid_base.err - pid->pid_base.last_err);

    pid->pid_base.last_err = pid->pid_base.err;

    pid->output = pid_output_limit(pid, output);

    return pid->output;
}

void pid_limit_set(pid_control_t *pid, pid_limit_t limit)
{
    ASSERT_NULL(pid);

    pid->limit = limit;
}

pid_limit_t pid_limit_get(pid_control_t *pid)
{
    ASSERT_NULL(pid);

    return pid->limit;
}

void pid_target_set(pid_control_t *pid, float target)
{
    ASSERT_NULL(pid);

    pid->target = target;
}

float pid_target_get(pid_control_t *pid)
{
    ASSERT_NULL(pid);

    return pid->target;
}
